/* Include files */
#include "math.h"
#include "mex.h"

/* Checks whether inputs are matrices */
#define IS_REAL_2D_FULL_DOUBLE(P) (!mxIsComplex(P) && \
mxGetNumberOfDimensions(P) == 2 && !mxIsSparse(P) && mxIsDouble(P))
#define IS_REAL_SCALAR(P) (IS_REAL_2D_FULL_DOUBLE(P) && mxGetNumberOfElements(P) == 1)

/*************************************************/
/* MAIN */
/*************************************************/

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/* Define output matrix */
	#define Bset    	plhs[0]
	#define FirmVec    	plhs[1]
	#define Share		plhs[2]
	#define nonzero		plhs[3]

	/* Define input matrices */
	#define rankA 	prhs[0]
	#define rankB   prhs[1]
	#define QbarA 	prhs[2]

	int a, b;

	/* Check the number of inputs is correct (Aoffer,Baccept,QbarA) */
	if(nrhs < 3)
		mexErrMsgTxt("Too few input arguments.");
	else if(nrhs > 3)
		mexErrMsgTxt("Too many input arguments.");

	if(!IS_REAL_2D_FULL_DOUBLE(rankA))
		mexErrMsgTxt("rankA must be real 2D full double array.");
	if(!IS_REAL_2D_FULL_DOUBLE(rankB))
		mexErrMsgTxt("rankB must be real 2D full double array.");
	if(!IS_REAL_2D_FULL_DOUBLE(QbarA))
		mexErrMsgTxt("QbarA must be real 2D full double array.");

	/* Dimensions and pointers for inputs */
	int A = mxGetM(rankA);
	int B = mxGetN(rankA);
	int nb = A*B;

	double *A_pt = mxGetPr(rankA);
	double *B_pt = mxGetPr(rankB);
	double *Q_pt = mxGetPr(QbarA);

	/* Output matrix */
	Bset = mxCreateDoubleMatrix(A,B,mxREAL);
	double *Bset_pt = mxGetPr(Bset);

	FirmVec = mxCreateDoubleMatrix(1,B,mxREAL);
	double *Firm_pt = mxGetPr(FirmVec);

	Share = mxCreateDoubleMatrix(A,1,mxREAL);
	double *Share_pt = mxGetPr(Share);

	nonzero = mxCreateDoubleMatrix(1,1,mxREAL);
	double *nonzero_pt = mxGetPr(nonzero);

	/* Uninitialized containers */
	mxArray *AConst = mxCreateDoubleMatrix(0,0,mxREAL);
	mxSetData(AConst, mxMalloc(sizeof(double)*A*1));
	double *AConst_pt = mxGetPr(AConst);

	mxArray *Bkeep = mxCreateDoubleMatrix(0,0,mxREAL);
	mxSetData(Bkeep, mxMalloc(sizeof(double)*1*B));
	double *Bkeep_pt = mxGetPr(Bkeep);

	  /* Generate index to use for sorting */
	int *FirmInd = malloc (sizeof (int) * B);
	int *FirmInd0 = malloc (sizeof (int) * B);
	int *Aindex = malloc (sizeof (int) * A);

	/* Initialize everything to zero otherwise crashes... not sure why */
	for(b = 0; b < B; b++)
	{
		FirmInd[b]  = -1;
		FirmInd0[b] = 0;
		Bkeep_pt[b] = 0;
		Firm_pt[b]  = 0;
	}
	for(a = 0; a < A; a++)
	{
		Aindex[a]    = 0;
		AConst_pt[a] = 0;
		Share_pt[a]  = 0;
	}

	/* Assignment test */
/*	printMatEl(A_pt,Bset_pt,A,B); */

/***************************************************************/
/* Begin Matching       */
/***************************************************************/
	int CheckEnd = 1;
	int diff = 1;
	int ind = 0;
	int indResp = 0;

	while(CheckEnd != 0) 
	{
		for(a = 0; a < A; a++)
		{
			/* Satisfies firm constraint to lease */
			diff = Q_pt[a] - AConst_pt[a];

			while ((0 < diff) && (Aindex[a] < B))
			{
				/* Parcel index for firms' preferences */
				ind = A_pt[a + A*Aindex[a]] - 1;  /* sequential firm rank of parcels */

				/* Offer was made to b in the sequence of B */
				Aindex[a] = Aindex[a] + 1;  /* Iterates the index (b) of offers for firms */

				/* Positive value for an offer constraint */
				if (ind > -1)
				{
					indResp = B_pt[a + A*ind];
					if (indResp > 0)
					{
						if (Bkeep_pt[ind] == 0)
						{
							Bkeep_pt[ind] = indResp;
							AConst_pt[a] = AConst_pt[a] + 1;
							FirmInd[ind] = a;
						} 
						else if (indResp < Bkeep_pt[ind])
						{
							AConst_pt[FirmInd[ind]] = AConst_pt[FirmInd[ind]] - 1;
							Bkeep_pt[ind] = indResp;
							AConst_pt[a] = AConst_pt[a] + 1;
							FirmInd[ind] = a;
						}		
					}	
				}			
				diff = Q_pt[a] - AConst_pt[a]; 
			}
		}

		CheckEnd = 0;
		for(b = 0; b < B; b++)
		{
			CheckEnd = CheckEnd + abs(FirmInd[b] - FirmInd0[b]);
			FirmInd0[b] = FirmInd[b];
		}
	}

	/* Allocate the matches to Bset */
	nonzero_pt[0] = 0;
	for(a = 0; a < A; a++)
	{
		for(b = 0; b < B; b++)
		{
			Bset_pt[a + A*b] = 0;
			if(FirmInd[b] == a)
			{
				Bset_pt[a + A*b] = 1;
				Firm_pt[b] = a + 1;
				Share_pt[a] = Share_pt[a] + 1;
			}
		}
		if (Share_pt[a] != 0)
		{
			Share_pt[a] = Share_pt[a]/B;
			nonzero_pt[0] = nonzero_pt[0] + 1;
		}
	}

	/* Check input */
/*  mexCallMATLAB(0,NULL,1, &Aoffer, "disp"); */

	mxDestroyArray(AConst);
	free(Aindex);
	mxDestroyArray(Bkeep);
	free(FirmInd);
	free(FirmInd0);

/*	return 0; */

}
